#include <stdio.h>

typedef struct {
	double*  vn;	// velocity in the navigation frame
	double*  rn;	// position in the navigation frame
	double*  q;		// quaternion
	double** Cbn;	// body-to-navigation frame conversion matrix
} STATE;

void _init();

void _destroy();

void _state_init(STATE *s, double rx, double ry, double rz, double vx, double vy, double vz, double phi, double theta, double psi);

void _state_destroy(STATE *s);

void _state_update(STATE *s, double *dtheta, double *dv, double dt);

void _state_print(STATE *s);

void _state_log_init(FILE* f);

void _state_log(FILE* f, double t, STATE *s);
